import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.MoveListener;

public class Robot {

	public static int x;
	public static int y;
	public static int direction;
	public static RobotHardware robothw;
	
	public static int numMovesAfterCorrection = 0;
	public static final int CORRECTION_LIMIT = 3; 

	public static void initialize(){
		robothw = new RobotHardware();
	}
	
	public static void setup(int x, int y, int direction){
			Robot.x = x;
			Robot.y = y;
			Robot.direction = direction;
			Data.currentFloor.grids[x][y].seen=1;
			updateGrid();
			detectObstacles();
	}

	public static void motionUpdate() {	

		robothw.goForward();
		numMovesAfterCorrection++;
		if(direction == Constants.RIGHT)
			x++;
		else if(direction == Constants.DOWN)
			y++;
		else if(direction == Constants.LEFT)
			x--;
		else if(direction == Constants.UP)
			y--;

		updateMoveList();

		///////READ SENSOR VALUES//////

		if(Data.currentFloor.grids[x][y].seen == 0) {
			updateGrid();
	//		System.out.println(x + " " + y  + "not seen");
		}

		Data.currentFloor.grids[x][y].seen++;
		detectObstacles();
		if(!(x==Data.firstFloor.xGrids - 1 && y==Data.firstFloor.yGrids - 1) && !(x==0 && y==Data.secondFloor.yGrids - 1))
			correctIfNeeded();
	}

	public static void detectObstacles(){
		Motor.B.rotateTo(0);
		double distance;
		int detectDist = 4 * Constants.GRID_SIZE / 5;

		distance = robothw.readUltrasonic();
		if(distance < detectDist){
			updateObstacle(0, direction, true);
		}
		Motor.B.rotateTo(90);
		distance = robothw.readUltrasonic();
		if(distance < detectDist){
			int dir = direction + 3;
//			if(direction == Constants.UP || direction == Constants.DOWN)
//				dir = direction + 1;
			updateObstacle(0, dir % 4, true);
		}
		Motor.B.rotateTo(-90);
		distance = robothw.readUltrasonic();
		if(distance < detectDist){
			int dir = direction + 1;
//			if(direction == Constants.UP || direction == Constants.DOWN)
//				dir = direction + 3;
			updateObstacle(0, dir % 4, true);
		}
		Motor.B.rotateTo(0);
	}

	public static void updateGrid() {
		Data.currentFloor.grids[x][y].color = ColorDetector.detectColor();
		int colorType = Data.currentFloor.grids[x][y].color;
		System.out.println("Color: "+colorType);
		if(colorType==Constants.FIRST_ROOM)
			Data.saysFirst++;
		else if(colorType==Constants.SECOND_ROOM)
			Data.saysSecond++;
		else if(colorType==Constants.KING && Data.currentFloor == Data.firstFloor){
			Data.kingSeen=true;
			Data.xKing = x;
			Data.yKing = y;
			Data.kingSeenMove = Data.moveList.size();
		}
	}

	public static void updateObstacle(int k, int dir, boolean exists){
	//	System.out.println("x: "+x+" y: "+y+" dir:" + dir);
		if(dir == Constants.RIGHT){
			Data.currentFloor.setObstacle(x + k, y, exists, true);
		}
		else if(dir == Constants.DOWN){
			Data.currentFloor.setObstacle(x, y + k, exists, false);
		}
		else if(dir == Constants.LEFT){
			Data.currentFloor.setObstacle(x - 1 - k, y, exists, true);
		}
		else if(dir == Constants.UP){
			Data.currentFloor.setObstacle(x, y - 1 - k, exists, false);
		}
	}

	public static void updateMoveList(){
		Data.moveList.add(new Move(direction));
	}

	public static void rotateTo(int heading){
		int diff = heading - direction;
		int degree = diff * 90;
		if(Math.abs(degree) == 270)
			degree = -degree / 3;
		if(degree != 0) {
			robothw.rotate(degree);
		}
		direction = heading;
		//detectObstacles();
	}
	
	public static void start(){
		robothw = new RobotHardware();
		int searchDegree = 30;
		int searchSize = 360 / searchDegree;
		double min = 1000; //big number
		int back = 0;
		int front;
		boolean corner = false;
		double distances[] = new double[searchSize];
		for(int i = 0; i < 360; i+=searchDegree){
			double dist = 0;
			try {
				Thread.sleep(100);
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
			for(int j = 0; j < 5; j++)
				dist += robothw.readUltrasonic();
			dist /= 5;
	//		System.out.println(dist);
			if(dist > 5 * Constants.GRID_SIZE)
				dist = 5 * Constants.GRID_SIZE;
			robothw.rotate(searchDegree);
			distances[i/searchDegree] = dist;

		}
		
		for(int i = 0; i < 360; i+=searchDegree){
			double dist = average(distances, i/searchDegree, searchSize);
			if(dist < min){
				min = dist;
				back = i;
			}
		}
		
		double dist[] = new double[4];
		dist[0] = average(distances, back/searchDegree, searchSize);
		dist[1] = average(distances, ((back/searchDegree + searchSize) + 90/searchDegree), searchSize);
		dist[2] = average(distances, ((back/searchDegree + searchSize) + 180/searchDegree), searchSize);
		dist[3] = average(distances, ((back/searchDegree + searchSize) + 270/searchDegree), searchSize);

		front = findFront(dist);
		int rotation = (back + front * 90 + 180) % 360;
		System.out.println(back);
		if(rotation > 180)
			robothw.rotate(rotation - 360);
		else
			robothw.rotate(rotation);
		robothw.correction(0);
		robothw.rotate(90);
		robothw.correction(0);
		robothw.rotate(-90);
		robothw.correction(0);
		robothw.rotate(180);
		robothw.pilot.setTravelSpeed(20);
			
	}
	
	private static int findFront(double[] dist){
		if(dist[1] - dist[0] <= 5){
			if(Math.abs(3.5 * Constants.GRID_SIZE - dist[3]) > Math.abs(3.5 * Constants.GRID_SIZE - dist[2]))
				return 3;
			else return 2;
		}
		else if(dist[3] - dist[0] <= 5){
			if(Math.abs(3.5 * Constants.GRID_SIZE - dist[1]) > Math.abs(3.5 * Constants.GRID_SIZE - dist[2]))
				return 1;
			else return 2;
		}
		else return 2;
	}
	
	private static void correctIfNeeded(){
		int prevDir = direction;
		if(numMovesAfterCorrection > CORRECTION_LIMIT){
			boolean obstacles[] = new boolean[4];
			Data.currentFloor.nearbyObstacles(obstacles, x, y);
			if(obstacles[0]&&obstacles[2])
				obstacles[2]=false;
			if(obstacles[1]&&obstacles[3])
				obstacles[3]=false;
			for(int i = 0; i < 4; i++){
				if(obstacles[i]){
					rotateTo(i);
					robothw.correction(0);
					numMovesAfterCorrection = 0;
				}
			}
		}
		rotateTo(prevDir);
	}
	
	private static double average(double[] distances, int i, int size){
		return (distances[i%size] + distances[(i+1)%size] + distances[(i-1+size)%size])/3;
		
	}
}